#include "obstacle_detector/util.h"

namespace obstacle_detector
{
//生成2行8列的包围框 前4列是底部包围框 顺时针排列 后4列是顶部包围框 顺时针排列
Eigen::MatrixXf generate_bbox(const Eigen::Vector3f &center,
	const Eigen::Vector3f &dim,float yaw)
{
	Eigen::MatrixXf bbox=Eigen::MatrixXf::Constant(3,8,0);
	Eigen::Vector3f corner_points;
	for (int i=0;i<2;i++)
	{
		corner_points(2)=(i*2-1)*dim(2)/2; //z
		for (int j=0;j<2;j++)
		{
			corner_points(1)=(j*2-1)*dim(1)/2; //y
			for (int k=0;k<2;k++)
			{
				corner_points(0)=(j*2-1)*(k*2-1)*dim(0)/2;//x
				bbox.col(i*4+j*2+k)=corner_points;
			}
		}
	}
	Eigen::Matrix3f rotation=Eigen::MatrixXf::Identity(3,3);
	rotation(0,0)=cos(yaw);
	rotation(0,1)=sin(yaw);
	rotation(1,0)=-1*sin(yaw);
	rotation(1,1)=cos(yaw);
	bbox=rotation*bbox;
	bbox=bbox.colwise()+center;
	return bbox;
}


void read_calib(std::string folder,float *all_transform)
{
	std::vector<std::string> calib_files;
	calib_files.push_back("front_left.txt");
	calib_files.push_back("front_right.txt");
	for (int i=0;i<calib_files.size();i++)
	{
		std::string trans_file=folder+"/"+calib_files[i];
		std::ifstream in(trans_file);
		std::string line;
		int row_count=0;
		Eigen::MatrixXf transform(4,4);
		// transform_.resize(3,4);
		while (getline (in, line))
		{
			// std::cout<<line<<std::endl;
			int col_count=0;
			std::istringstream tokenStream(line);
			std::string token;
			while(getline(tokenStream, token, ' '))
			{
				transform(row_count,col_count)=std::stof(token);
				col_count++;
			}
			row_count++;
		}
		transform(3,0)=0;
		transform(3,1)=0;
		transform(3,2)=0;
		transform(3,3)=1;

		for (int m=0;m<3;m++)
		{
			for (int n=0;n<4;n++)
			{
				all_transform[i*12+m*4+n]=transform(m,n);
			}
		}
		std::cout<<transform<<std::endl;
	}
}

void save_points(const int16_t* points, const uint16_t* points_order,
	int point_num,const module_base::Location &loc)
{
	std::string file_name="/home/westwell/welldriver/motion.txt";
	std::ofstream out(file_name);
	std::string content=std::to_string(loc.x)+" "+std::to_string(loc.y)+" "+std::to_string(loc.yaw)+"\n";
	out<<content;
	for (int i=0;i<point_num;i++)
	{
		std::string content=std::to_string(points[i*4+0])+" ";
		content+=std::to_string(points[i*4+1])+" ";
		content+=std::to_string(points[i*4+2])+" ";
		content+=std::to_string(points[i*4+3])+" ";
		content+=std::to_string(points_order[i*2+0])+" ";
		content+=std::to_string(points_order[i*2+1])+"\n";
		out<<content;
	}

	out.close();
}

void save_points(const float* points, int point_num,
	double stamp)
{
	std::string file_name="/home/westwell/welldriver/"+std::to_string(stamp)+".txt";
	std::ofstream out(file_name);
	for (int i=0;i<point_num;i++)
	{
		std::string content=std::to_string(points[i*4+0])+" ";
		content+=std::to_string(points[i*4+1])+"\n";
		out<<content;
	}

	out.close();

}

void read_points(int16_t* points, uint16_t* points_order,int &point_num,
	module_base::Location &loc)
{
	std::string file_name="/home/westwell/welldriver/motion.txt";
	std::ifstream in(file_name);
	std::string line;
	getline (in, line);
	std::istringstream tokenStream(line);
	std::string token;
	int count=0;
	float values[3];
	while(getline(tokenStream, token, ' '))
	{
		values[count]=std::stof(token);
		count++;
	}
	loc.x=values[0];
	loc.y=values[1];
	loc.yaw=values[2];


	int num=0;
	while (getline (in, line))
	{
		std::istringstream tokenStream(line);
		std::string token;
		int count=0;
		while(getline(tokenStream, token, ' '))
		{
			if (count<4)
			{
				points[num*4+count]=std::stoi(token);
			}
			else
			{
				points_order[num*2+count-4]=std::stoi(token);
			}
			count++;
		}
		num++;
	}
	// point_num=num;
	// printf("num: %d\n",num);
	in.close();
}

void save_bbox(std::vector<Object> &all_objects,
	std::string file_name)
{
	std::string save_name="/home/westwell/welldriver/"+file_name;
	std::ofstream out(save_name);
	for (int i=0;i<all_objects.size();i++)
	{
		std::string content="";
		content+=(std::to_string(all_objects[i].label)+" ");
		for (int j=0;j<all_objects[i].bbox.cols();j++)
		{
			content+=(std::to_string(all_objects[i].bbox(0,j))+" ");
			content+=(std::to_string(all_objects[i].bbox(1,j))+" ");			
		}
		for (int j=0;j<all_objects[i].global_bbox.cols();j++)
		{
			content+=(std::to_string(all_objects[i].global_bbox(0,j))+" ");
			content+=(std::to_string(all_objects[i].global_bbox(1,j))+" ");
		}
		content+="\n";
		out<<content;
	}
	out.close();
}

void read_bbox(std::vector<Object> &all_objects,
	std::string file_name)
{
	std::string read_name="/home/westwell/welldriver/"+file_name;
	std::ifstream in(read_name);
	std::string line;
	all_objects.clear();
	while (getline (in, line))
	{
		std::istringstream tokenStream(line);
		std::string token;
		int count=0;
		int label;
		Eigen::MatrixXf bbox(2,8);
		Eigen::MatrixXf global_bbox(2,8);
		while(getline(tokenStream, token, ' '))
		{
			if (count==0) 
			{
				label=std::stoi(token);
				count++;
				continue;
			}
			float value=std::stof(token);
			int row_index=(count-1)%2;
			int col_index=(count-1)/2;
			if (col_index<8)
			{
				bbox(row_index,col_index)=value;
			}
			else
			{
				global_bbox(row_index,col_index-8)=value;
			}
			count++;
		}
		std::string id="test";
		all_objects.push_back(Object(id,label,bbox,global_bbox));
	}
}

void save_occupancy_grid(const module_base::Location &loc,
	uint8_t *occupancy_grid,int x_size,int y_size)
{
	std::string save_name="/home/westwell/welldriver/occupancy_grid.txt";
	std::ofstream out(save_name);
	std::string content=std::to_string(loc.x)+" "+std::to_string(loc.y)+" "+std::to_string(loc.yaw)+"\n";
	out<<content;
	for (int i=0;i<x_size;i++)
	{
		for (int j=0;j<y_size;j++)
		{
			uint8_t value=occupancy_grid[i*y_size+j];
			if (value==0)
			{
				continue;
			}
			std::string content=std::to_string(i)+" "+std::to_string(j)+" "+std::to_string(value)+"\n";
			out<<content;
		}
	}
	out.close();
}

void read_occupancy_grid(module_base::Location &loc,
	uint8_t *occupancy_grid,int x_size,int y_size)
{
	std::string file_name="/home/westwell/welldriver/occupancy_grid.txt";
	std::ifstream in(file_name);
	std::string line;
	getline (in, line);
	std::istringstream tokenStream(line);
	std::string token;
	int count=0;
	float values[3];
	while(getline(tokenStream, token, ' '))
	{
		values[count]=std::stof(token);
		count++;
	}
	loc.x=values[0];
	loc.y=values[1];
	loc.yaw=values[2];

	while (getline (in, line))
	{
		// std::cout<<line<<std::endl;
		std::istringstream tokenStream(line);
		std::string token;
		int count=0;
		int index[2];
		while(getline(tokenStream, token, ' '))
		{
			// printf("count %d: %s\n",count,token.c_str());
			if (count<2)
			{
				index[count]=std::stoi(token);
			}
			else
			{
				occupancy_grid[index[0]*y_size+index[1]]=std::stoi(token);
				// printf ("value %d,%d: %d,%s\n",index[0],index[1],
				// 	occupancy_grid[index[0]*y_size+index[1]],token.c_str());
			}
			count++;
		}
	}
	// point_num=num;
	// printf("num: %d\n",num);
	in.close();
}


void get_color(int label,float *color)
{
	color[0]=0;
	color[1]=0;
	color[2]=0;
	if (label==0) color[0]=1;
	else if (label==1) color[1]=1;
	else if (label==2) color[2]=1;
	else if (label==3) {
		color[0]=1;
		color[1]=1;
	}
	else if (label==4)
	{
		color[0]=1;
		color[2]=1;
	}
	else if (label==5)
	{
		color[1]=1;
		color[2]=1;
	}
	else if (label==6)
	{
		color[0]=0.5;
		color[1]=0.5;
	}
	else if (label==8)
	{
		color[0]=0.5;
		color[2]=0.5;
	}
	else
	{
		color[1]=0.5;
		color[2]=0.5;
	}

}

}